
#include "link.h"
#include <vector>
#include <Eigen/Core>

using namespace Eigen;
using namespace std;


#ifndef LINKMANAGER_H
#define LINKMANAGER_H
class LinkManager{
	public:
		
		LinkManager();
		~LinkManager();
		
		vector<Link*> links;
		Vector3f origin;
		
		void initLink(Vector3f o, Vector3f axis, float t, float l);
		void display();
		void addLink(Vector3f axis, float t,  float l);
		
		Vector3f getPos(Link* l, Vector3f v);
		Vector3f getLinkEnd(Vector3f axis, float t, float l, int linkNum);
		Matrix<float, 4, 4> getT(float length);
		Matrix<float, 4, 4> getComposedMatrix(Vector3f axis, float theta,  float length);
		
		Vector3f pi(const Matrix<float, 12, 1>& v, Vector3f end);
		Matrix<float, 3, 12> get3D3LJacobian(const Matrix<float, 12, 1>& v);
		Vector3f p(const Matrix<float, 12, 1>& v);
	
		void calculate(Vector3f end);
		
		
	/*	
		Vector3f getEndPos();
		
		Matrix<float, 2, 3> get2D3LJacobian(float theta1, float theta2, float theta3);
		Matrix<float, 3, 6> get3D3LJacobian(float theta1, float theta2, float theta3, float phi1, float phi2, float phi3);
		
		void draw(Vector3f pos, Vector3f end);
		//void draw(Vector3f pos, float theta, float phi, float length);
		Vector3f p(float theta1, float theta2, float theta3, float phi1, float phi2, float phi3);
		Vector3f pi(Matrix<float, 6, 1> v, Vector3f end);		
	    void drawCube(float x , float y , float z);
		Matrix<float, 4, 4> getRy(float theta);
		Matrix<float, 4, 4> getRz(float theta);
		Matrix<float, 4, 4> getT(float theta, float phi, float length);*/
};
#endif